#include <complex>
#include <math.h>
#include <iostream>

#include <QDebug>

#include "roaddata.h"
#include "../algorithm/spiral/spiral.h"

using namespace std;


#define PI acos(-1)
#define STR_RAIL "rail"
#define STR_RESTRICTED "restricted"
#define STR_STOP "stop"
#define STR_NONE "none"
#define STR_BORDER "border"
#define STR_BIKING "biking"
#define STR_PARKING "parking"
#define STR_SIDEWALK "sidewalk"
#define STR_DRIVING "driving"
#define STR_EXIT "exit"
#define STR_ENTRY "entry"
#define STR_SHOULDER "shoulder"
#define STR_STANDARD "standard"
#define STR_SOLID "solid"
#define STR_BROKEN "broken"

RoadData::RoadData()
{

    //车道线和填充颜色的定义
    m_qmapColorLineStyle["none"] = transparent;
    m_qmapColorLineStyle["driving"] = gray;
    m_qmapColorLineStyle["sidewalk"] = lightGray;
    m_qmapColorLineStyle["border"] = transparent;
    m_qmapColorLineStyle["standard"] = white;

    //车道线线型的定义
    m_qmapColorLineStyle["broken"] = DashLine;
    m_qmapColorLineStyle["solid"] = SolidLine;
//  qmapColorLineStyle["none"] = NoPen;  //如果车道标志类型为none,依然需要画出的时候,创建两个map,各自定义两个map的key寻找value函数
}

RoadData::~RoadData()
{
}

void RoadData::getGeometryLineData(double sOffset, double dstartds, double dendds, double dstepSize, Geometry &geometry, LaneOffset &laneOffset, Lane &lane, QVector<QVector<QPointF>> &lanePoint)
{
    double s = sOffset;
    double xStart = geometry.x;
    double yStart = geometry.y;
    double dhdg = geometry.hdg;
    double dlaneId = lane.iId;
    LaneWidth laneWidth = lane.structWidth;

    double dsgn = sgn(dlaneId);
    QVector<QPointF> vecGeometryLine;
    QPointF qpointf;

    if (lanePoint.size() == 0)
    {
        double dlaneOffset = 0;
        for(double ds = dstartds, i = 0; ds < dendds; ds += dstepSize, ++i)
        {
            dlaneOffset = laneOffset.a + laneOffset.b * (s+i) + laneOffset.c * pow((s+i), 2) + laneOffset.d * pow((s+i), 3);
            qpointf.setX(xStart + ds * cos(dhdg) - dlaneOffset * sin(dhdg));
            qpointf.setY(yStart + ds * sin(dhdg) + dlaneOffset * cos(dhdg));
            vecGeometryLine.push_back(qpointf);
        }

        double ds = dendds - dstartds;
        dlaneOffset = laneOffset.a + laneOffset.b * (s+ds) + laneOffset.c * pow((s+ds), 2) + laneOffset.d * pow((s+ds), 3);
        qpointf.setX(xStart + dendds * cos(dhdg) - dlaneOffset * sin(dhdg));
        qpointf.setY(yStart + dendds * sin(dhdg) + dlaneOffset * cos(dhdg));
        vecGeometryLine.push_back(qpointf);
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> prelane = lanePoint[lanePoint.size() - 1];
        if (isZero(laneWidth.a) && isZero(laneWidth.b) && isZero(laneWidth.c) && isZero(laneWidth.d))
        {
            lanePoint.push_back(prelane);
            return;
        }
        uint size = prelane.size();
        double width = 0;
        for(uint i = 0; i < size - 1; ++i)
        {
            width = laneWidth.a + laneWidth.b * (s+i) + laneWidth.c * pow((s+i), 2) + laneWidth.d * pow((s+i), 3);
            qpointf.setX(prelane[i].x() - dsgn * width * sin(dhdg));
            qpointf.setY(prelane[i].y() + dsgn * width * cos(dhdg));
            vecGeometryLine.push_back(qpointf);
        }

        double ds = dendds - dstartds;
        width = laneWidth.a + laneWidth.b * (s+ds) + laneWidth.c * pow((s+ds), 2) + laneWidth.d * pow((s+ds), 3);
        qpointf.setX(prelane[size-1].x() - dsgn * width * sin(dhdg));
        qpointf.setY(prelane[size-1].y() + dsgn * width * cos(dhdg));
        vecGeometryLine.push_back(qpointf);
    }

    lanePoint.push_back(vecGeometryLine);
}

//道路几何元素为spiral类型的定义,传入指定参数,返回元素为QPoint类型的QVector
QVector<QPointF> RoadData::getGeometrySpiralData(double xStart, double yStart, double hdgStart, double xEnd, double yEnd, double hdgEnd, double width, double dLaneId)
{
    double dSgnId = sgn(dLaneId);

    //螺旋线近似为直线,得出开始、结束点的对应点
    QPointF qpointStartCorrespond, qpointEndCorrespond;

    qpointStartCorrespond.setX(xStart - dSgnId * width * sin(hdgStart));
    qpointStartCorrespond.setY(yStart + dSgnId * width * cos(hdgStart));

    qpointEndCorrespond.setX(xEnd - dSgnId * width * sin(hdgEnd));
    qpointEndCorrespond.setY(yEnd + dSgnId * width * cos(hdgEnd));

    QVector<QPointF> qvecGeometrySpiral;
    qvecGeometrySpiral.push_back(qpointStartCorrespond);
    qvecGeometrySpiral.push_back(qpointEndCorrespond);

    return qvecGeometrySpiral;
}

QVector<QPointF> RoadData::getGeometrySpiralData(double xStart, double yStart, double xEnd, double yEnd)
{
    QPointF qstartPoint, qendPoint;

    qstartPoint.setX(xStart);
    qstartPoint.setY(yStart);

    qendPoint.setX(xEnd);
    qendPoint.setY(yEnd);

    QVector<QPointF> qvecGeometrySpiral;
    qvecGeometrySpiral.push_back(qstartPoint);
    qvecGeometrySpiral.push_back(qendPoint);

    return qvecGeometrySpiral;
}

QVector<QPointF> RoadData::getGeometryNewSpiralData(double x, double y, double dhdg, double dcurvStart, double dcurvEnd, double dlength, double dwidth, double dlaneId)
{
    double dsgnId = sgn(dlaneId);

    double cDelta = 0.0;
    if (dlength <= std::numeric_limits<double>::epsilon() && dlength >= -std::numeric_limits<double>::epsilon())
    {
        cDelta = 0.0;
    }
    else
    {
        cDelta = 1 * (dcurvEnd - dcurvStart) / dlength;
    }

    // 螺旋线
    // 直线
    // 标准螺旋线
    // 曲线arc

    // Start
    complex<double> myComplex(1,1);
    auto C0 = x + myComplex.real() * y;

    Spiral spiral;
    QVector<QPointF> qvecGeometrySpiral;
    QPointF qpointStartCorrespond;
    qpointStartCorrespond.setX(x - dsgnId * dwidth * sin(dhdg));
    qpointStartCorrespond.setY(y + dsgnId * dwidth * cos(dhdg));

    for(int s = 0; s <= dlength; s++)
    {
        double hdgCurrent = 0.0;
        double tx = 0.0;
        double ty = 0.0;
        if ( (cDelta <= 1e-8 && cDelta >= -1e-8) and (dcurvStart <= 1e-8 && dcurvStart >= -1e-8) )
        {
            // Straight line
            std::complex<double> Cs = C0 + std::exp(myComplex.real() * dhdg * s);
            tx = Cs.real();
            ty = Cs.imag();
        }
        else if ( (cDelta <= 1e-8 && cDelta >= -1e-8) and !(dcurvStart <= 1e-8 && dcurvStart >= -1e-8))
        {
            // Arc
            std::complex<double> Cs = C0 +
                                      std::exp(myComplex.real() * dhdg) /
                                      dcurvStart *
                                      ( sin(dcurvStart * s) + myComplex.real() * (1 - cos(dcurvStart * s)) );
            tx = Cs.real();
            ty = Cs.imag();
        }
        else
        {
            // 1)
            // Fresnel integrals
            double fresnelA = (dcurvStart + cDelta * s) / sqrt(PI * fabs(cDelta));
            //double fresnelB = curvStart / sqrt(PI * fabs(cDelta));

            //Euler Spiral
            spiral.calculateSpiral(fresnelA, cDelta, &ty, &tx, &hdgCurrent);
            complex<double> complexA(tx,ty);
            //double Ca = hdgCurrent;
//            spiral.odrSpiral(fresnelB, cDelta, &y, &x, &hdgCurrent);
//            complex<double> complexB(x,y);
            //double Cb = hdgCurrent;

//            std::complex<double> Cs1 = sqrt(PI / fabs(cDelta)) * exp(
//                myComplex.real() * (hdgStart - curvStart * curvStart / 2 / cDelta)
//            );
//            std::complex<double> Cs2 = sgn(cDelta) * (Ca - Cb) + myComplex.real() * complexA - myComplex.real() * complexB;
//            std::complex<double> Cs = C0 + Cs1 * Cs2;
//            x = Cs.real();
//            y = Cs.imag();
            // 2)
            //spiral.odrSpiral(s, cDelta, &y, &x, &hdgCurrent);
        }

        hdgCurrent = cDelta * s * s * 0.5 + dcurvStart * s + dhdg;
        QPointF qpoint;
        qpoint.setX(x + tx - dsgnId * dwidth * sin(hdgCurrent));
        qpoint.setY(y + ty + dsgnId * dwidth * cos(hdgCurrent));
        qvecGeometrySpiral.push_back(qpoint);
    }

    return qvecGeometrySpiral;
}

void RoadData::getGeometryArcData(double sOffset, double dstartds, double dendds, double dstepSize, Geometry &geometry, LaneOffset &laneOffset, Lane &lane, QVector<QVector<QPointF>> &lanePoint)
{
    double s = sOffset;
    double xStart = geometry.x;
    double yStart = geometry.y;
    double dhdg = geometry.hdg;
    double dcurvature = geometry.extend.a;
    double dlaneId = lane.iId;
    LaneWidth laneWidth = lane.structWidth;

    double dsgnId = sgn(dlaneId);
    double R = -1 / dcurvature;

    QVector<QPointF> qvecGeometryArc;
    double doffsetX;
    double doffsetY = 0;
    double dcurrentX = 0;
    double dcurrentY = 0;
    double dhdgOffset = 0;
    double dhdgCorrespond = 0;
    double width = 0;
    QPointF qpoint;

    if (lanePoint.size() == 0)
    {
        for(double ds = dstartds; ds < dendds; ds += dstepSize)
        {
            doffsetX = R * (sin(dhdg) - sin(dhdg - ds / R));  //x方向偏移
            doffsetY = R * (cos(dhdg - ds / R) - cos(dhdg));  //y方向偏移

            dcurrentX = xStart + doffsetX; // x坐标
            dcurrentY = yStart + doffsetY; // y坐标

            qpoint.setX(dcurrentX);
            qpoint.setY(dcurrentY);
            qvecGeometryArc.push_back(qpoint);
        }

        // 终点坐标
        doffsetX = R * (sin(dhdg) - sin(dhdg - dendds / R));  //x方向偏移
        doffsetY = R * (cos(dhdg - dendds / R) - cos(dhdg));  //y方向偏移

        dcurrentX = xStart + doffsetX; // x坐标
        dcurrentY = yStart + doffsetY; // y坐标

        qpoint.setX(dcurrentX);
        qpoint.setY(dcurrentY);
        qvecGeometryArc.push_back(qpoint);
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> prelane = lanePoint[lanePoint.size() - 1];
        if (isZero(laneWidth.a) && isZero(laneWidth.b) && isZero(laneWidth.c) && isZero(laneWidth.d))
        {
            lanePoint.push_back(prelane);
            return;
        }

        uint size = prelane.size();
        for(uint i = 0; i < size - 1; ++i)
        {
            double ds = dstartds+i;
            dhdgOffset = ds / R;  //角度的偏移量
            dhdgCorrespond = dhdg - dhdgOffset;  //点的切角大小
            width = laneWidth.a + laneWidth.b * (s+ds) + laneWidth.c * pow((s+ds), 2) + laneWidth.d * pow((s+ds), 3);
            dcurrentX = prelane[i].x() - dsgnId * width * sin(dhdgCorrespond);  //求弧线上点的对应点的x坐标
            dcurrentY = prelane[i].y() + dsgnId * width * cos(dhdgCorrespond);  //求弧线上点的对应点的y坐标

            qpoint.setX(dcurrentX);
            qpoint.setY(dcurrentY);
            qvecGeometryArc.push_back(qpoint);
        }

        double ds = dendds - dstartds;
        width = laneWidth.a + laneWidth.b * (s+ds) + laneWidth.c * pow((s+ds), 2) + laneWidth.d * pow((s+ds), 3);

        dhdgOffset = ds / R;  //角度的偏移量
        dhdgCorrespond = dhdg - dhdgOffset;  //点的切角大小
        dcurrentX = prelane[size - 1].x() - dsgnId * width * sin(dhdgCorrespond);  //求弧线上点的对应点的x坐标
        dcurrentY = prelane[size - 1].y() + dsgnId * width * cos(dhdgCorrespond);  //求弧线上点的对应点的y坐标

        qpoint.setX(dcurrentX);
        qpoint.setY(dcurrentY);
        qvecGeometryArc.push_back(qpoint);
    }

    lanePoint.push_back(qvecGeometryArc);

}

void RoadData::getGeometryPoly3Data(double sOffset, double dstartds, double dendds, double dstepSize, Geometry &geometry, Lane &lane, QVector<QVector<QPointF>> &lanePoint)
{
    double xStart = geometry.x;
    double yStart = geometry.y;
    double a = geometry.extend.a;
    double b = geometry.extend.b;
    double c = geometry.extend.c;
    double d = geometry.extend.d;
    double dhdg = geometry.hdg;
    double dlaneId = lane.iId;
    LaneWidth laneWidth = lane.structWidth;

    double dsgnId = sgn(dlaneId);
    QVector<QPointF> qvecGeometryPoly;

    // 叉乘
    double dnewb = b*a;
    double dnewc = c*b;
    double dnewd = d*c;
    QPointF qpoint;

    if (lanePoint.size() == 0)
    {
        double dvLocal = 0;
        double dsChange = 0;
        double dtChange = 0;
        double dpolyX = 0;
        double dpolyY = 0;
        for(double ds = dstartds; ds < dendds; ds += dstepSize)
        {
            dvLocal = a + b * ds + c * pow(ds, 2) + d * pow(ds, 3);
            // s和t进行旋转平移
            dsChange = ds * cos(dhdg) - dvLocal * sin(dhdg);
            dtChange = ds * sin(dhdg) + dvLocal * cos(dhdg);

            // 参考线坐标
            dpolyX = xStart + dsChange;
            dpolyY = yStart + dtChange;

            qpoint.setX(dpolyX);
            qpoint.setY(dpolyY);
            qvecGeometryPoly.push_back(qpoint);
        }

        dvLocal = a + b * dendds + c * pow(dendds, 2) + d * pow(dendds, 3);
        // s和t进行旋转平移
        dsChange = dendds * cos(dhdg) - dvLocal * sin(dhdg);
        dtChange = dendds * sin(dhdg) + dvLocal * cos(dhdg);

        // 参考线坐标
        dpolyX = xStart + dsChange;
        dpolyY = yStart + dtChange;

        qpoint.setX(dpolyX);
        qpoint.setY(dpolyY);
        qvecGeometryPoly.push_back(qpoint);
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> prelane = lanePoint[lanePoint.size() - 1];
        if (isZero(laneWidth.a) && isZero(laneWidth.b) && isZero(laneWidth.c) && isZero(laneWidth.d))
        {
            lanePoint.push_back(prelane);
            return;
        }

        double width = 0;
        double dtangent = 0;
        double droadMarkX = 0;
        double droadMarkY = 0;
        uint size = prelane.size();
        for(uint i = 0; i < size - 1; ++i)
        {
            // 方向变化
            double ds = dstartds + i;
            dtangent = dnewb + dnewc * ds + dnewd * pow(ds, 2);

            // 车道线处位置
            width = laneWidth.a + laneWidth.b * ds + laneWidth.c * pow(ds, 2) + laneWidth.d * pow(ds, 3);
            droadMarkX = prelane[i].x() - dsgnId * width * sin(dhdg + dtangent);
            droadMarkY = prelane[i].y() + dsgnId * width * cos(dhdg + dtangent);

            qpoint.setX(droadMarkX);
            qpoint.setY(droadMarkY);
            qvecGeometryPoly.push_back(qpoint);
        }

        double ds = dendds - dstartds;
        width = laneWidth.a + laneWidth.b * ds + laneWidth.c * pow(ds, 2) + laneWidth.d * pow(ds, 3);

        // 方向变化
        dtangent = dnewb + dnewc * ds + dnewd * pow(ds, 2);

        droadMarkX = prelane[size - 1].x() - dsgnId * width * sin(dhdg + dtangent);
        droadMarkY = prelane[size - 1].y() + dsgnId * width * cos(dhdg + dtangent);

        qpoint.setX(droadMarkX);
        qpoint.setY(droadMarkY);
        qvecGeometryPoly.push_back(qpoint);

    }

    lanePoint.push_back(qvecGeometryPoly);
}

//符号函数的定义,传入一个变量,如果变量为正数,返回1;如果变量为负数,返回-1;如果变量为0,返回0
double RoadData::sgn(double dSgnVar)
{
    double dSgnValue;

    if (dSgnVar > 0.0)
    {
        dSgnValue = 1;
    }
    else if (dSgnVar < 0.0)
    {
        dSgnValue = -1;
    }
    else if (dSgnVar <= std::numeric_limits<double>::epsilon() && dSgnVar >= -std::numeric_limits<double>::epsilon())
    {
        dSgnValue = 0;
    }

    return dSgnValue;
}

template<typename T>

bool RoadData::isZero(T value)
{
    if (value <= std::numeric_limits<T>::epsilon() && value >= -std::numeric_limits<T>::epsilon())
        return true;
    else
        return false;
}

//QVector<QPointF>的合并
void RoadData::mergeQVectorQPointF(QVector<QPointF> &qvecDesPoint, const QVector<QPointF> &qvecSrcPoint)
{
    for (int i = 0; i < qvecSrcPoint.size(); ++i)
    {
        qvecDesPoint.push_back(qvecSrcPoint[i]);
    }

//    for (int i=0;i<qvec_main_body.size();++i)
//    {
//        qDebug() << qvec_main_body[i].rx() << qvec_main_body[i].ry(); //输出测试
//    }
}

//车道填充数据的定义
QVector<QPointF> RoadData::fillLaneData(const QVector<QPointF> &qvecOutsideLaneData, const QVector<QPointF> &qvecInsideLaneData)
{
    QVector<QPointF> qvecLaneFillingData;
    for(int i = 0; i < qvecOutsideLaneData.size(); ++i)
    {
        qvecLaneFillingData.push_back(qvecOutsideLaneData[i]);
    }

    for(int i = qvecInsideLaneData.size() - 1; i >= 0; --i)
    {
        qvecLaneFillingData.push_back(qvecInsideLaneData[i]);
    }

//    for(int i=0;i<qvec_lane_filling_data.size();++i)
//    {
//        qDebug() << qvec_lane_filling_data[i].rx() << qvec_lane_filling_data[i].ry(); //输出测试
//    }

    return qvecLaneFillingData;
}

//寻找Map的key对应的value的定义
int RoadData::key2Value(const QString &strK)
{
    return m_qmapColorLineStyle.value(strK);
}

//y的坐标取相反数
void RoadData::setYOppositeNumber(QVector<QPointF> &qvecData)
{
    for(int i=0; i < qvecData.size(); ++i)
    {
        qvecData[i].setY( - qvecData[i].ry());
    }
}

QPolygonF RoadData::getYOppositeNumber(const QPolygonF &polygon)
{
    QPolygonF oppositePolygon;
    oppositePolygon = polygon;
    setYOppositeNumber(oppositePolygon);
    return oppositePolygon;
}

QColor RoadData::getColor(QString qstrType)
{
    if(m_qmapColor.isEmpty())
    {
        m_qmapColor[STR_RAIL] = QColor(190, 147, 120);
        m_qmapColor[STR_RESTRICTED] = QColor(133, 152, 103);
        m_qmapColor[STR_STOP] = QColor(106, 90, 90);
        m_qmapColor[STR_NONE] = QColor(190, 147, 120);
        m_qmapColor[STR_BORDER] = QColor(70, 82, 122);
        m_qmapColor[STR_BIKING] = QColor(104, 200, 172);
        m_qmapColor[STR_PARKING] = QColor(96, 145, 212);
        m_qmapColor[STR_SIDEWALK] = QColor(188, 193, 203);
        m_qmapColor[STR_DRIVING] = QColor(160, 160, 160);
        m_qmapColor[STR_EXIT] = QColor(165, 175, 194);
        m_qmapColor[STR_ENTRY] = QColor(224, 192, 195);
        m_qmapColor[STR_SHOULDER] = QColor(154, 199, 163);
        m_qmapColor[STR_STANDARD] = Qt::white;
    }

    return m_qmapColor.find(qstrType) == m_qmapColor.end() ? Qt::color0 : m_qmapColor.value(qstrType);
}

Qt::PenStyle RoadData::getPenStyle(QString qstrType)
{
    if(m_qmapPenStyle.isEmpty())
    {
        m_qmapPenStyle[STR_NONE] = Qt::NoPen;
        m_qmapPenStyle[STR_BROKEN] = Qt::DashLine;
        m_qmapPenStyle[STR_SOLID] = Qt::SolidLine;
    }

    return m_qmapPenStyle.find(qstrType) == m_qmapPenStyle.end() ? Qt::NoPen : m_qmapPenStyle.value(qstrType);
}
